/***************************************************************************
 * @version   --> v1.0
 * @date      -->2021-04-18
 * @brief     -->姿态管理模块
 *            1.通过串口接收上位机发送的指令，主要控制四个飞轮和三轴磁力矩器，
 *              从而调节姿态，同时把当前立方星的姿态信息通过串口发送给上位机
 *            2.通过Arduino串口调试时，可以调节飞轮转速和控制磁力矩器产生的力矩
 *@update     -->
 **************************************************************************/

/*Includes --------------------------------------------------------------*/
#include <Wire.h>       // IIC通信
#include <SCoop.h>      // 使用SCoop线程库
#include <string.h>     // 使用内存拷贝
#include "IMUGY85.h"    // 姿态敏感器
#include <MsTimer.h>    // 使用定时器

/* Private macro --------------------------------------------------------*/
// 宏开关，可启用Serial用于串口调试
// 正式编译的时候务必注释掉
//#define SERIAL_DEBUG

/* Private define ------------------------------------------------------*/
// 引脚定义
const int WHEEL_DIR0 = 7;   // 1号电机方向控制引脚 7
const int WHEEL_FG0 = 8;    // 1号电机霍尔信号引脚 8
const int WHEEL_PWM0 = 9;   // 1号电机转速控制引脚 9
const int WHEEL_DIR1 = A9;  // 2号电机方向控制引脚 A9
const int WHEEL_FG1 = A8;   // 2号电机霍尔信号引脚 A8
const int WHEEL_PWM1 = 6;   // 2号电机转速控制引脚 6
const int WHEEL_DIR2 = 40;  // 3号电机方向控制引脚 40
const int WHEEL_FG2 = 41;   // 3号电机霍尔信号引脚 41
const int WHEEL_PWM2 = 45;  // 3号电机转速控制引脚 45
const int WHEEL_DIR3 = 42;  // 4号电机方向控制引脚 42
const int WHEEL_FG3 = 43;   // 4号电机霍尔信号引脚 43
const int WHEEL_PWM3 = 44;  // 4号电机方向控制引脚 44

const int MAG_IN0 = A10;    // A磁矩控制引脚 A10
const int MAG_IN1 = A11;    // A磁矩控制引脚 A11
const int MAG_PWMA = 5;     // A磁矩使能引脚 5
const int MAG_IN2 = A12;    // B磁矩控制引脚 A12
const int MAG_IN3 = A13;    // B磁矩控制引脚 A13
const int MAG_PWMB = 2;     // B磁矩使能引脚 2
const int MAG_IN4 = A14;    // C磁矩控制引脚 A14
const int MAG_IN5 = A15;    // C磁矩控制引脚 A15
const int MAG_PWMC = 3;     // C磁矩控制引脚 3

IMUGY85 imu;      // IMU（九轴）传感器

// 串口数据缓冲区
// 单次收发数据帧在？字节以内
uint8_t send_buffer[];
uint8_t receiver_buffer[];

// ISR（中断服务程序）中所涉及的变量需要被声明为volatile易变变量
// 状态标志量，飞轮控制，磁力矩器
volatile bool wheel_on = false;
volatile bool magnet_on = false;

// 飞轮转速记录
volatile int16_t speed_wheel1_measurement = 0;  // 飞轮1的转速测量值，-32768~32767
volatile int16_t speed_wheel1_setting = 0;      // 飞轮1的转速设定值，
volatile bool wheel1_forward = true;             // 飞轮1正转
uint16_t wheel1_counter = 0;                    // 飞轮1编码计数器
volatile int16_t speed_wheel2_measurement = 0;  // 飞轮2的转速测量值，-32768~32767
volatile int16_t speed_wheel2_setting = 0;      // 飞轮2的转速设定值，
volatile bool wheel2_forward = true;             // 飞轮2正转
uint16_t wheel2_counter = 0;                    // 飞轮2编码计数器
volatile int16_t speed_wheel3_measurement = 0;  // 飞轮3的转速测量值，-32768~32767
volatile int16_t speed_wheel3_setting = 0;      // 飞轮3的转速设定值，
volatile bool wheel3_forward = true;             // 飞轮3正转
uint16_t wheel3_counter = 0;                    // 飞轮3编码计数器
volatile int16_t speed_wheel4_measurement = 0;  // 飞轮4的转速测量值，-32768~32767
volatile int16_t speed_wheel4_setting = 0;      // 飞轮4的转速设定值，
volatile bool wheel4_forward = true;             // 飞轮4正转
uint16_t wheel4_counter = 0;                    // 飞轮4编码计数器

// 磁矩设置值
volatile uint8_t x_magnetmoment_setting = 0;
volatile uint8_t y_magnetmoment_setting = 0;
volatile uint8_t z_magnetmoment_setting = 0;
volatile bool x_magnet_forward = true;    // X轴磁矩正向
volatile bool y_magnet_forward = true;    // Y轴磁矩正向      
volatile bool z_magnet_forward = true;    // Z轴磁矩正向

// IMU数据
float eulers[3] = {0};
float angular_velocities[3] = {0};
float accelerations[3] = {0};

// 速度转换函数
int16_t measurement(uint16_t wheel_counter, bool wheel_forward){
  int16_t speed_wheel_measurement = wheel_counter/9/0.5*60;
  if(!wheel_forward){
    speed_wheel_measurement = -speed_wheel_measurement
  }
#ifdef SERIAL_DEBUG
  Serial.print("当前转速：");
  Serial.print(speed_wheel_measurement);
  Serial.println("r/min");
#endif
  return speed_wheel_measurement;
}

// 中断函数


void setup() {
  // put your setup code here, to run once:

}

void loop() {
  // put your main code here, to run repeatedly:

}
